#include <string.h>
#include <float.h>
#include <string>
#include "posemath.h"
#include "rtapi/rtapi.h"
#include "hal/hal.h"
#include "motion.h"
#include "motion_debug.h"
#include "motion_struct.h"
#include "mot_priv.h"
#include "rtapi/rtapi_math.h"
#include "nml_intf/motion_types.h"
#include "errcode.h"
#include "motion/semintf.h"
#include "dyna/jaka_dyn_intf.h"
#include "kinematics/kinematics.h"
#include "modbuscrc.h"
//#include "tp/tpmanager.hh"
#include "motion/motionItf.h"
#include "zuc/nml_intf/zucpos.h"
#include <sys/select.h>
#include "timemonitor.h"
#include "torque_control_queue.h"
#include "log/zuclog.h"
#include "rtbus/rtbus.h"
#include "rtdev/safetyboard/scbitf.h"
#include "rtdev/tio_intf_c.h"
#include "rtdev/rs485msgque.h"

#define MOTCMD_INFO(fmt, ...) zuclog_info("MOTCMD", fmt, ##__VA_ARGS__)
#define MOTCMD_ERR(fmt, ...) zuclog_error("MOTCMD", fmt, ##__VA_ARGS__)
#define MOTCMD_DEBUG(fmt, ...) zuclog_debug("MOTCMD", "[%06d] " fmt, __LINE__, ##__VA_ARGS__)

#define ABS(x) (((x) < 0) ? -(x) : (x))
// Mark strings for translation, but defer translation to userspace
#define _(s) (s)
#define REFIT_NUM 6

#define MAX_ERROR_MSG_STR_LEN 256

static int rehomeAll;

static KINEMATICS_FORWARD_FLAGS temp_fflags_flange;
static KINEMATICS_INVERSE_FLAGS temp_iflags_flange;
static KINEMATICS_FORWARD_FLAGS temp_fflags;
static KINEMATICS_INVERSE_FLAGS temp_iflags;
static double saved_rapid_rate = 1.0;
static int newConfigTemp = 255;
static int task_init_finish_flag = 0;
double coef_lpf = 1.0;
double jpos_lpf[ZUCMOT_MAX_JOINTS] = {0};
//static double jointPos[ZUCMOT_MAX_JOINTS] = {0};

#define REDUCE_MODE_TCP_VEL_LIMIT 250.0
#define REDUCE_MODE_TCP_ABC_VEL_LIMIT 90.safe

void firstOrder_LPF(double* outJPos, double* inJPos, double beta)
{
    int i = 0;
    if (outJPos == 0 || inJPos == 0)
    {
        return;
    }
    for (i = 0; i < zucmotConfig->numJoints; i++) { outJPos[i] = beta * inJPos[i] + (1 - beta) * outJPos[i]; }
}

/* loops through the active joints and checks if any are not homed */
int checkAllHomed(void)
{
    int joint_num;
    zucmot_joint_t* joint;

    for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    {
        /* point to joint data */
        joint = &joints[joint_num];
        if (!GET_JOINT_ACTIVE_FLAG(joint))
        {
            /* if joint is not active, don't even look at its limits */
            continue;
        }
        if (!GET_JOINT_HOMED_FLAG(joint))
        {
            /* if any of the joints is not homed return false */
            return 0;
        }
    }
    /* return true if all active joints are homed*/
    return 1;
}

/* limits_ok() returns 1 if none of the hard limits are set,
 0 if any are set. Called on a linear and circular move. */
static int limits_ok(void)
{
    int joint_num;
    zucmot_joint_t* joint;

    for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    {
        /* point to joint data */
        joint = &joints[joint_num];
        if (!GET_JOINT_ACTIVE_FLAG(joint))
        {
            /* if joint is not active, don't even look at its limits */
            continue;
        }

        if (GET_JOINT_PHL_FLAG(joint) || GET_JOINT_NHL_FLAG(joint))
        {
            return 0;
        }
        if ((joint->pos_cmd > joint->max_pos_limit && fabs(joint->pos_cmd - joint->max_pos_limit) > 1e-3) ||
            (joint->pos_cmd < joint->min_pos_limit && fabs(joint->pos_cmd - joint->min_pos_limit) > 1e-3))
        {
            return 0;
        }
    }

    return 1;
}

static int check_axis_constraint(double target, int id, const char* move_type, int axis_no)
{
    int in_range = 1;
    double nl = axes[axis_no].min_pos_limit;
    double pl = axes[axis_no].max_pos_limit;

    if (target < nl)
    {
        in_range = 0;
        reportError(AXIS_EXCEED_NEG_LIMIT, "move on line %d would exceed axis %d negative limit", id, axis_no);
    }

    if (target > pl)
    {
        in_range = 0;
        reportError(AXIS_EXCEED_POS_LIMIT, "move on line %d would exceed axis %d positive limit", id, axis_no);
    }

    return in_range;
}

/* inRange() returns non-zero if the position lies within the joint
 limits, or 0 if not.  It also reports an error for each joint limit
 violation.  It's possible to get more than one violation per move. */
static int inRange(ZucPose pos, int id, const char* move_type)
{
    // double joint_pos[ZUCMOT_MAX_JOINTS];
    // int joint_num;
    // zucmot_joint_t *joint;
    int in_range = 1;

    if (in_range && check_axis_constraint(pos.tran.x, id, move_type, 0) == 0)
        in_range = 0;
    if (in_range && check_axis_constraint(pos.tran.y, id, move_type, 1) == 0)
        in_range = 0;
    if (in_range && check_axis_constraint(pos.tran.z, id, move_type, 2) == 0)
        in_range = 0;
    if (in_range && check_axis_constraint(pos.a, id, move_type, 3) == 0)
        in_range = 0;
    if (in_range && check_axis_constraint(pos.b, id, move_type, 4) == 0)
        in_range = 0;
    if (in_range && check_axis_constraint(pos.c, id, move_type, 5) == 0)
        in_range = 0;
    // if(check_axis_constraint(pos.u, id, move_type, 6) == 0)
    //     in_range = 0;
    // if(check_axis_constraint(pos.v, id, move_type, 7) == 0)
    //     in_range = 0;
    // if(check_axis_constraint(pos.w, id, move_type, 8) == 0)
    //     in_range = 0;

    /* Now, check that the endpoint puts the joints within their limits too */

    /* fill in all joints with 0 */

    // for (joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++) {
    // 	joint_pos[joint_num] = 0;
    // }

    // for (joint_num = 0; joint_num < zucmotConfig->numJoints /*zucmotConfig->numJoints*/; joint_num++) {
    // 	/* point to joint data */
    // 	joint = &joints[joint_num];

    // 	if (!GET_JOINT_ACTIVE_FLAG(joint)) {
    // 		/* if joint is not active, don't even look at its limits */
    // 		continue;
    // 	}

    // 	if (!isfinite(joint_pos[joint_num])) {
    // 		reportError(KINEMATICS_FAILED, _("%s move on line %d gave non-finite joint location on joint %d"), move_type, id, ONE_BASED(joint_num));
    // 		in_range = 0;
    // 		continue;
    // 	}
    // 	if (joint_pos[joint_num] > joint->max_pos_limit) {
    // 		in_range = 0;
    // 		reportError(JOINT_HIT_POS_SLIMIT_J1 + (joint_num<<16), _("%s move on line %d would exceed joint %d's positive limit"), move_type, id, ONE_BASED(joint_num));
    // 	}

    // 	if (joint_pos[joint_num] < joint->min_pos_limit) {
    // 		in_range = 0;
    // 		reportError(JOINT_HIT_NEG_SLIMIT_J1 + (joint_num<<16), _("%s move on line %d : %f would exceed joint %d's negative limit: %f"), move_type, id, joint_pos[joint_num], ONE_BASED(joint_num), joint->min_pos_limit);
    // 	}
    // }
    return in_range;
}

// @FIXME liwang 7轴
// joint pos in ZucPose

/* clearHomes() will clear the homed flags for joints that have moved
 since homing, outside coordinated control, for machines with no
 forward kinematics. This is used in conjunction with the rehomeAll
 flag, which is set for any coordinated move that in general will
 result in all joints moving. The flag is consulted whenever a joint
 is jogged in joint mode, so that either its flag can be cleared if
 no other joints have moved, or all have to be cleared. */
void clearHomes(int joint_num)
{
    int n;
    zucmot_joint_t* joint;

    if (zucmotConfig->kinType == KINEMATICS_INVERSE_ONLY)
    {
        if (rehomeAll)
        {
            for (n = 0; n < zucmotConfig->numJoints; n++)
            {
                /* point at joint data */
                joint = &(joints[n]);
                /* clear flag */
                SET_JOINT_HOMED_FLAG(joint, 0);
            }
        }
        else
        {
            /* point at joint data */
            joint = &joints[joint_num];
            /* clear flag */
            SET_JOINT_HOMED_FLAG(joint, 0);
        }
    }
}

/*
 zucmotCommandHandler() is called each main cycle to read the
 shared memory buffer
 */
extern void zucmotCommandHandler2(void* arg);
void zucmotCommandHandler(void* arg)
{
    canmonitor_record_timer(48);
    zucmotCommandHandler2(arg);
    canmonitor_record_timer(49);
}
void zucmotCommandHandler2(void* arg)
{
    int joint_num, axis_num;
    int n;
    zucmot_joint_t* joint;
    zucmot_axis_t* axis;
    double tmp1;
    zucmot_comp_entry_t* comp_entry;
    char issue_atspeed = 0;
    int abort = 0;

    EndCond condition;
    //AppVersion = zucmotCommand->motsetADout.appver;
    /* check for split read */
    if (zucmotCommand->head != zucmotCommand->tail)
    {
        zucmotDebug->split++;
        return; /* not really an error */
    }

    if (zucmotCommand->commandNum != zucmotStatus->commandNumEcho)
    {
        /* increment head count-- we'll be modifying zucmotStatus */
        zucmotStatus->head++;
        zucmotDebug->head++;

        /* got a new command-- echo command and number... */
        zucmotStatus->commandEcho = zucmotCommand->command;
        zucmotStatus->commandNumEcho = zucmotCommand->commandNum;

        /* clear status value by default */
        zucmotStatus->commandStatus = ZUCMOT_COMMAND_OK;

        /* ...and process command */
        joint = 0;
        axis = 0;

        /* HAVE A QUESTION HERE */
        joint_num = zucmotCommand->joint;  //选用哪个关节
        axis_num = zucmotCommand->axis;    //选用哪个轴

        //-----------------------------------------------------------------------------
        // joints_axes test for unexpected conditions
        // example: non-cooperating guis
        // example: attempt to jog locking indexer axis letter
        std::string emsg;
        if (zucmotCommand->command == ZUCMOT_JOG_CONT || zucmotCommand->command == ZUCMOT_JOG_INCR || zucmotCommand->command == ZUCMOT_JOG_ABS)
        {
            // joint_num = zucmotCommand->jog.joint; //选用哪个关节
            // axis_num = zucmotCommand->jog.axis;	  //选用哪个轴
            if ((GET_MOTION_TELEOP_FLAG() || GET_MOTION_TELEOP_TOOL_FLAG()) && axis_num < 0)
            {
                emsg = "command teleop bad axis_num";
                abort = 1;
            }
            if (abort == 0 && !(GET_MOTION_TELEOP_FLAG() || GET_MOTION_TELEOP_TOOL_FLAG()) && joint_num < 0)
            {
                emsg = "command teleop bad joint_num";
                abort = 1;
            }
            if (GET_MOTION_TELEOP_FLAG() || GET_MOTION_TELEOP_TOOL_FLAG())
            {
                axis = &axes[axis_num];
                if ((axis_num >= 0) && (axis->locking_joint >= 0))
                {
                    MOTCMD_ERR("Cannot jog a locking indexer AXIS_%c,joint_num=%d\n", "XYZABCUVW"[axis_num], axis -> locking_joint);
                    zucmotStatus->tail = zucmotStatus->head;
                    zucmotConfig->tail = zucmotConfig->head;
                    zucmotDebug->tail = zucmotDebug->head;
                    return;
                }
            }
        }

        if (abort)
        {
            if (!limits_ok())
            {
                reportError(ROBOT_ON_SOFT_LIMIT, _("Can't jog axis %d when when it's on soft limit."), ONE_BASED(axis_num));
            }
            switch (zucmotCommand->command)
            {
            case ZUCMOT_JOG_CONT:
                MOTCMD_ERR("JOG_CONT %s cmd=%d\n", emsg.c_str(), zucmotCommand->command);
                break;
            case ZUCMOT_JOG_INCR:
                MOTCMD_ERR("JOG_INCR %s cmd=%d\n", emsg.c_str(), zucmotCommand->command);
                break;
            case ZUCMOT_JOG_ABS:
                MOTCMD_ERR("JOG_ABS %s cmd=%d\n", emsg.c_str(), zucmotCommand->command);
                break;
            default:
                break;
            }
            zucmotStatus->tail = zucmotStatus->head;
            zucmotConfig->tail = zucmotConfig->head;
            zucmotDebug->tail = zucmotDebug->head;
            return;
        }

        if (joint_num >= 0 && joint_num < zucmotConfig->numJoints)
        {
            joint = &joints[joint_num];
        }
        if (axis_num >= 0 && axis_num < ZUCMOT_MAX_AXIS)
        {
            axis = &axes[axis_num];
        }
        switch (zucmotCommand->command)
        {
        case ZUCMOT_TRACKING_ENABLE: {
            // MOTCMD_INFO(" #################################################################### ZUCMOT_TRACKING_ENABLE #################################################################### \n");
            zucmotStatus->tracking = 1;
            zucmotStatus->still_dec = 0;
            zucmotStatus->pre_singular = 0;
            zucmotStatus->maxdis_judge = 0;
            zucmotStatus->conveyorMaxDistance = zucmotCommand->enaconvyrtrack.max_distance;
            zucmotStatus->conveyorPulseEquivalent = zucmotCommand->enaconvyrtrack.conveyorPulseEquivalent;
            zucmotStatus->last_vel_for_convy = 0.0;
            zucmotStatus->first_cycle_in_tracking = 1;
            zucmotStatus->status_acc_convyr = 0;
            zucmotStatus->last_theta = 0.0;
            zucmotStatus->convyr_type = zucmotCommand->enaconvyrtrack.convyr_type;

            if (zucmotCommand->enaconvyrtrack.convyr_type == 1)
            {
                //                tpManager_tpSetTrackLinearCfgEna(1, zucmotCommand->enaconvyrtrack.conveyorPulseEquivalent, zucmotCommand->enaconvyrtrack.conveyorDir);
                // zucmotStatus->conveyorPulseEquivalent = zucmotCommand->enaconvyrtrack.conveyorPulseEquivalent;
                // zucmotStatus->conveyorDir.x = zucmotCommand->enaconvyrtrack.conveyorDir.x;
                // zucmotStatus->conveyorDir.y = zucmotCommand->enaconvyrtrack.conveyorDir.y;
                // zucmotStatus->conveyorDir.z = zucmotCommand->enaconvyrtrack.conveyorDir.z;
            }
            else if (zucmotCommand->enaconvyrtrack.convyr_type == 2)
            {
                //                tpManager_tpSetTrackCircularCfgEna(1,
                //    zucmotCommand->enaconvyrtrack.conveyorPulseEquivalent,
                //    zucmotCommand->enaconvyrtrack.o_to_world,
                //    zucmotCommand->enaconvyrtrack.rotate_tool);
                //    zucmotStatus->conveyorPulseEquivalent = zucmotCommand->enaconvyrtrack.conveyorPulseEquivalent;
                //    zucmotStatus->o_to_world = zucmotCommand->enaconvyrtrack.o_to_world;
                //    zucmotStatus->rotate_tool = zucmotCommand->enaconvyrtrack.rotate_tool;
            }
            if (zucmotStatus->conveyorMaxDistance < 1e-5)
            {
                zucmotStatus->ValidDistance = 0;
            }
            else
            {
                zucmotStatus->ValidDistance = 1;
            }
            //            if (tpManager_GetConveyAnyChanged() != 0 && tpManager_GetConveyEnabled())
            {  // we have DIO's to turn on or off
                if (zucmotCommand->enaconvyrtrack.convyr_type == 1)
                {
                    //                    tpManager_GetConveyDir(&zucmotStatus->conveyorDir);
                    pmCartUnitEq(&zucmotStatus->conveyorDir);
                    if (!zucmotConfig->lctsynch)
                    {
                        zucmotStatus->status_acc_convyr = 3;
                    }
                }
                else if (zucmotCommand->enaconvyrtrack.convyr_type == 2)
                {
                    //                    tpManager_GetConveyO2world(&zucmotStatus->o_to_world);
                    zucmotStatus->r_for_convyr = 0.0;
                    //                    zucmotStatus->rotate_tool = tpManager_GetConveyRotTool();

                    // MOTCMD_INFO("[tpToggleTrackCfg] ---> zucmotStatus->o_to_world : \n");
                    // MOTCMD_INFO("zucmotStatus->o_to_world.rot = : \n");
                    // MOTCMD_INFO("%f %f %f\n", zucmotStatus->o_to_world.rot.x.x, zucmotStatus->o_to_world.rot.y.x, zucmotStatus->o_to_world.rot.z.x);
                    // MOTCMD_INFO("%f %f %f\n", zucmotStatus->o_to_world.rot.x.y, zucmotStatus->o_to_world.rot.y.y, zucmotStatus->o_to_world.rot.z.y);
                    // MOTCMD_INFO("%f %f %f\n", zucmotStatus->o_to_world.rot.x.z, zucmotStatus->o_to_world.rot.y.z, zucmotStatus->o_to_world.rot.z.z);
                }
                if (fabs(zucmotStatus->conveyorPulseEquivalent) < 1e-8)
                {
                    zucmotStatus->status_acc_convyr = 3;
                }
                if (zucmotConfig->link_di < 0 || zucmotConfig->link_di > 8 || !zucmotStatus->get_conveyor_ori)
                {
                    long int convayorCnt = zucmotStatus->last_convayorCnt;
                    MOTCMD_INFO(
                        "-------------  zucmotConfig->link_di = %d, [ZUCMOT_TRACKING_ENABLE]------------:convayorCnt = %ld, conveyorPulseEquivalent = %f\n ",
                        zucmotConfig->link_di,
                        convayorCnt,
                        zucmotStatus->conveyorPulseEquivalent);
                    zucmotStatus->turns_count = 0;
                    zucmotStatus->conveyorOrigin = convayorCnt;
                    zucmotStatus->last_convayorCnt = convayorCnt;
                    zucmotStatus->uhiData.pulses = zucmotStatus->conveyorOrigin;
                    zucmotStatus->get_conveyor_ori = 1;
                }
            }

            if (zucmotConfig->link_di < 0)
            {
                long int convayorCnt = zucmotStatus->last_convayorCnt;
                MOTCMD_INFO(
                    "-------------  zucmotConfig->link_di = %d, [ZUCMOT_TRACKING_ENABLE]------------:convayorCnt = %ld, conveyorPulseEquivalent = %f\n ",
                    zucmotConfig->link_di,
                    convayorCnt,
                    zucmotStatus->conveyorPulseEquivalent);
                zucmotStatus->turns_count = 0;
                zucmotStatus->conveyorOrigin = convayorCnt;
                zucmotStatus->last_convayorCnt = convayorCnt;
                zucmotStatus->uhiData.pulses = zucmotStatus->conveyorOrigin;
            }
            MOTCMD_INFO(" ---------------------------------------- [ZUCMOT_TRACKING_ENABLE] ---------------------------------------- "
                        ":zucmotStatus->last_convayorCnt =%ld, zucmotStatus->uhiData.pulses = %ld\n",
                        zucmotStatus->last_convayorCnt,
                        zucmotStatus->uhiData.pulses);

            // if (get_robot_joint_collision_sensitivity() != 0x0F)
            // {
            //     set_robot_joint_collision_sensitivity(0x0F);
            // }
            break;
        }

        case ZUCMOT_TRACKING_DISABLE: {
            MOTCMD_INFO(" ******************************************************** TRACKING DISABLE...\n");
            // reportInfo(INFO_CONVEYOR_TRACK_DISABLED, "conveyor track function disabled");
            // temp_fflags = ctrl_fflags;
            // temp_iflags = ctrl_iflags;
            // /* set conveyor tracking cfg */
            // zucmotStatus->tracking = 0;
            //            // tpManager_tpDisableTrack();
            // zucmotStatus->conveyorPulseEquivalent = 0;
            // zucmotStatus->status_acc_convyr = 3;

            // /* get real joint position according to joints */
            // int jntNum = 0;
            // double jointPos[ZUCMOT_MAX_JOINTS] = {0}, tar_pos_joint[ZUCMOT_MAX_JOINTS] = {0};
            // for (jntNum = 0; jntNum < zucmotConfig->numJoints; jntNum++)
            // {
            //     jointPos[jntNum] = joints[jntNum].pos_cmd;
            //     tar_pos_joint[jntNum] = joints[jntNum].pos_cmd;
            // }
            // /* get the real carte_pos by kinematic forward */
            // //KINEMATICS_FORWARD_FLAGS l_fflags = 0;
            // //KINEMATICS_INVERSE_FLAGS l_iflags = 0;
            // kinematicsForward(&zucmotConfig->tool_offset,
            //                   jointPos,
            //                   &zucmotStatus->disable_convyr_real_pos,
            //                   &ctrl_fflags,
            //                   &ctrl_iflags,
            //                   &zucmotConfig->base_offset,
            //                   &zucmotConfig->user_offset);
            // //$1(ctrl_fflags);
            // //zucmotSet_CtrIFLAGES(ctrl_iflags);
            // int result = 0;
            // ZucPose tar_pos;
            // tar_pos = zucmotStatus->disable_convyr_real_pos;  //init tar_pos
            // if ((zucmotStatus->convyr_type == 1 && zucmotConfig->lctsynch) || zucmotStatus->convyr_type == 2)
            // {
            //     if (zucmotStatus->convyr_type == 2)
            //     {  //若是圆形传送带跟踪，则求减速至零转过的角度并根据旋转变换求速度为零时的笛卡尔和关节位置
            //         if (zucmotStatus->r_for_convyr != 0.0)
            //         {
            //             zucmotStatus->left_theta = pmSq(zucmotStatus->last_vel_for_convy) / 2.0 / zucmotStatus->r_for_convyr /
            //                                        zucmotConfig->synch_acc;  //zucmotStatus->r_for_convyr==0???,
            //         }
            //         else
            //         {
            //             zucmotStatus->left_theta = 0.0;
            //         }
            //     }
            //     else
            //     {
            //         zucmotStatus->left_theta = pmSq(zucmotStatus->last_vel_for_convy) / 2.0 / zucmotConfig->synch_acc;  //v^2 = 2*a*s
            //     }

            //     /* get the pos when the vel(omega) of robot is zero */
            //     get_tar_pos(zucmotStatus->left_theta, &tar_pos);
            //     result = kinematicsInverse(
            //         &zucmotConfig->tool_offset, &tar_pos, tar_pos_joint, &ctrl_iflags, &ctrl_fflags, &zucmotConfig->base_offset, &zucmotConfig->user_offset);
            //     MOTCMD_INFO(" ######################## ZUCMOT_TRACKING_DISABLE left_theta = %f ########################\n", zucmotStatus->left_theta);
            //     MOTCMD_INFO("tar_pos = %f %f %f %f %f %f\n, tar_pos_joint = %f %f %f %f %f %f\n",
            //                 tar_pos.tran.x,
            //                 tar_pos.tran.y,
            //                 tar_pos.tran.z,
            //                 tar_pos.a,
            //                 tar_pos.b,
            //                 tar_pos.c,
            //                 tar_pos_joint[0],
            //                 tar_pos_joint[1],
            //                 tar_pos_joint[2],
            //                 tar_pos_joint[3],
            //                 tar_pos_joint[4],
            //                 tar_pos_joint[5]);
            //     if (result != 0 || (zucmotStatus->convyr_type == 2 && zucmotStatus->r_for_convyr < 1e-8))
            //     {
            //         // for (jntNum = 0; jntNum < zucmotConfig->numJoints; jntNum++) { zucmotDebug->coord_tp.currentPos_joint[jntNum] = jointPos[jntNum]; }
            //            //         tpManager_tpSetCurJpos(jointPos);
            //         // tpSetPos(&zucmotDebug->coord_tp, &zucmotStatus->disable_convyr_real_pos, jointPos);
            //            //         tpManager_tpSetPos_JPos(&zucmotStatus->disable_convyr_real_pos, jointPos);
            //         // zucmotStatus->carte_pos_desired = zucmotDebug->coord_tp.currentPos;
            //            //         tpManager_tpGetPose(&zucmotStatus->carte_pos_desired);
            //         zucmotStatus->still_dec = 0;
            //     }
            //     else
            //     {
            //            //         tpManager_tpSetPos_JPos(&tar_pos, tar_pos_joint);
            //         zucmotStatus->still_dec = 1;
            //     }
            // }

            // else
            // {
            //     // for (jntNum = 0; jntNum < zucmotConfig->numJoints; jntNum++) { zucmotDebug->coord_tp.currentPos_joint[jntNum] = jointPos[jntNum]; }
            //     // tpSetPos(&zucmotDebug->coord_tp, &zucmotStatus->disable_convyr_real_pos, jointPos);
            //     // zucmotStatus->carte_pos_desired = zucmotDebug->coord_tp.currentPos;

            //            //     tpManager_tpSetCurJpos(jointPos);
            //            //     tpManager_tpSetPos_JPos(&zucmotStatus->disable_convyr_real_pos, jointPos);
            //            //     tpManager_tpGetPose(&zucmotStatus->carte_pos_desired);
            //     zucmotStatus->still_dec = 0;
            // }
            // if (get_robot_joint_collision_sensitivity() == 0x0F)
            // {
            //     // in case of unexpected exit from admittance or drag
            //     set_robot_joint_collision_sensitivity(zucmotConfig->safetyCfg.clsn_sensitivity);
            // }
            break;
        }

        case ZUCMOT_SET_FUNCDI_MAP: {
            /*only update di map  */
            zucmotStatus->funcIOMap.task_run_di = zucmotCommand->setfuncDIO.funcIOMap.task_run_di;
            zucmotStatus->funcIOMap.task_pause_di = zucmotCommand->setfuncDIO.funcIOMap.task_pause_di;
            zucmotStatus->funcIOMap.task_resume_di = zucmotCommand->setfuncDIO.funcIOMap.task_resume_di;
            zucmotStatus->funcIOMap.task_stop_di = zucmotCommand->setfuncDIO.funcIOMap.task_stop_di;
            zucmotStatus->funcIOMap.power_on_di = zucmotCommand->setfuncDIO.funcIOMap.power_on_di;
            zucmotStatus->funcIOMap.power_off_di = zucmotCommand->setfuncDIO.funcIOMap.power_off_di;
            zucmotStatus->funcIOMap.servo_enable_di = zucmotCommand->setfuncDIO.funcIOMap.servo_enable_di;
            zucmotStatus->funcIOMap.servo_disable_di = zucmotCommand->setfuncDIO.funcIOMap.servo_disable_di;
            zucmotStatus->funcIOMap.percentage_mode_level1_di = zucmotCommand->setfuncDIO.funcIOMap.percentage_mode_level1_di;
            zucmotStatus->funcIOMap.stop_mode_di = zucmotCommand->setfuncDIO.funcIOMap.stop_mode_di;
            zucmotStatus->funcIOMap.back_to_inipos_di = zucmotCommand->setfuncDIO.funcIOMap.back_to_inipos_di;
            zucmotStatus->funcIOMap.percentage_mode_level2_di = zucmotCommand->setfuncDIO.funcIOMap.percentage_mode_level2_di;
            zucmotStatus->funcIOMap.clear_error_di = zucmotCommand->setfuncDIO.funcIOMap.clear_error_di;  //
            zucmotStatus->funcIOMap.drag_mode_di = zucmotCommand->setfuncDIO.funcIOMap.drag_mode_di;
            zucmotStatus->funcIOMap.drag_mode_disable_di = zucmotCommand->setfuncDIO.funcIOMap.drag_mode_disable_di;
            break;
        }

        case ZUCMOT_SET_FUNCDO_MAP: {  //
            int i = 0;
            /*only update do map*/
            for (i = 0; i < 4; i++)
            {
                // task_idle_do
                if (zucmotCommand->setfuncDIO.funcIOMap.task_idle_do[i].type >= 0 && zucmotCommand->setfuncDIO.funcIOMap.task_idle_do[i].index >= 0)
                {
                    FD_SET(zucmotCommand->setfuncDIO.funcIOMap.task_idle_do[i].index,
                           &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotCommand->setfuncDIO.funcIOMap.task_idle_do[i].type]);
                }
                else
                {
                    if (zucmotStatus->funcIOMap.task_idle_do[i].type >= 0 && zucmotStatus->funcIOMap.task_idle_do[i].index >= 0)
                    {
                        FD_CLR(zucmotStatus->funcIOMap.task_idle_do[i].index,
                               &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotStatus->funcIOMap.task_idle_do[i].type]);
                    }
                }
                // task_paused_do
                if (zucmotCommand->setfuncDIO.funcIOMap.task_paused_do[i].type >= 0 && zucmotCommand->setfuncDIO.funcIOMap.task_paused_do[i].index >= 0)
                {
                    FD_SET(zucmotCommand->setfuncDIO.funcIOMap.task_paused_do[i].index,
                           &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotCommand->setfuncDIO.funcIOMap.task_paused_do[i].type]);
                }
                else
                {
                    if (zucmotStatus->funcIOMap.task_paused_do[i].type >= 0 && zucmotStatus->funcIOMap.task_paused_do[i].index >= 0)
                    {
                        FD_CLR(zucmotStatus->funcIOMap.task_paused_do[i].index,
                               &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotStatus->funcIOMap.task_paused_do[i].type]);
                    }
                }
                // task_running_do
                if (zucmotCommand->setfuncDIO.funcIOMap.task_running_do[i].type >= 0 && zucmotCommand->setfuncDIO.funcIOMap.task_running_do[i].index >= 0)
                {
                    FD_SET(zucmotCommand->setfuncDIO.funcIOMap.task_running_do[i].index,
                           &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotCommand->setfuncDIO.funcIOMap.task_running_do[i].type]);
                }
                else
                {
                    if (zucmotStatus->funcIOMap.task_running_do[i].type >= 0 && zucmotStatus->funcIOMap.task_running_do[i].index >= 0)
                    {
                        FD_CLR(zucmotStatus->funcIOMap.task_running_do[i].index,
                               &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotStatus->funcIOMap.task_running_do[i].type]);
                    }
                }
                // stat_fault_do
                if (zucmotCommand->setfuncDIO.funcIOMap.stat_fault_do[i].type >= 0 && zucmotCommand->setfuncDIO.funcIOMap.stat_fault_do[i].index >= 0)
                {
                    FD_SET(zucmotCommand->setfuncDIO.funcIOMap.stat_fault_do[i].index,
                           &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotCommand->setfuncDIO.funcIOMap.stat_fault_do[i].type]);
                }
                else
                {
                    if (zucmotStatus->funcIOMap.stat_fault_do[i].type >= 0 && zucmotStatus->funcIOMap.stat_fault_do[i].index >= 0)
                    {
                        FD_CLR(zucmotStatus->funcIOMap.stat_fault_do[i].index,
                               &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotStatus->funcIOMap.stat_fault_do[i].type]);
                    }
                }
                // stat_poweron_do
                if (zucmotCommand->setfuncDIO.funcIOMap.stat_poweron_do[i].type >= 0 && zucmotCommand->setfuncDIO.funcIOMap.stat_poweron_do[i].index >= 0)
                {
                    FD_SET(zucmotCommand->setfuncDIO.funcIOMap.stat_poweron_do[i].index,
                           &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotCommand->setfuncDIO.funcIOMap.stat_poweron_do[i].type]);
                }
                else
                {
                    if (zucmotStatus->funcIOMap.stat_poweron_do[i].type >= 0 && zucmotStatus->funcIOMap.stat_poweron_do[i].index >= 0)
                    {
                        FD_CLR(zucmotStatus->funcIOMap.stat_poweron_do[i].index,
                               &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotStatus->funcIOMap.stat_poweron_do[i].type]);
                    }
                }
                // stat_enabled_do
                if (zucmotCommand->setfuncDIO.funcIOMap.stat_enabled_do[i].type >= 0 && zucmotCommand->setfuncDIO.funcIOMap.stat_enabled_do[i].index >= 0)
                {
                    FD_SET(zucmotCommand->setfuncDIO.funcIOMap.stat_enabled_do[i].index,
                           &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotCommand->setfuncDIO.funcIOMap.stat_enabled_do[i].type]);
                }
                else
                {
                    if (zucmotStatus->funcIOMap.stat_enabled_do[i].type >= 0 && zucmotStatus->funcIOMap.stat_enabled_do[i].index >= 0)
                    {
                        FD_CLR(zucmotStatus->funcIOMap.stat_enabled_do[i].index,
                               &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotStatus->funcIOMap.stat_enabled_do[i].type]);
                    }
                }
                // stat_motion_do
                if (zucmotCommand->setfuncDIO.funcIOMap.stat_motion_do[i].type >= 0 && zucmotCommand->setfuncDIO.funcIOMap.stat_motion_do[i].index >= 0)
                {
                    FD_SET(zucmotCommand->setfuncDIO.funcIOMap.stat_motion_do[i].index,
                           &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotCommand->setfuncDIO.funcIOMap.stat_motion_do[i].type]);
                }
                else
                {
                    if (zucmotStatus->funcIOMap.stat_motion_do[i].type >= 0 && zucmotStatus->funcIOMap.stat_motion_do[i].index >= 0)
                    {
                        FD_CLR(zucmotStatus->funcIOMap.stat_motion_do[i].index,
                               &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotStatus->funcIOMap.stat_motion_do[i].type]);
                    }
                }
                // stat_static_do
                if (zucmotCommand->setfuncDIO.funcIOMap.stat_static_do[i].type >= 0 && zucmotCommand->setfuncDIO.funcIOMap.stat_static_do[i].index >= 0)
                {
                    FD_SET(zucmotCommand->setfuncDIO.funcIOMap.stat_static_do[i].index,
                           &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotCommand->setfuncDIO.funcIOMap.stat_static_do[i].type]);
                }
                else
                {
                    if (zucmotStatus->funcIOMap.stat_static_do[i].type >= 0 && zucmotStatus->funcIOMap.stat_static_do[i].index >= 0)
                    {
                        FD_CLR(zucmotStatus->funcIOMap.stat_static_do[i].index,
                               &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotStatus->funcIOMap.stat_static_do[i].type]);
                    }
                }
                // stat_boot_do
                if (zucmotCommand->setfuncDIO.funcIOMap.stat_boot_do[i].type >= 0 && zucmotCommand->setfuncDIO.funcIOMap.stat_boot_do[i].index >= 0)
                {
                    FD_SET(zucmotCommand->setfuncDIO.funcIOMap.stat_boot_do[i].index,
                           &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotCommand->setfuncDIO.funcIOMap.stat_boot_do[i].type]);
                }
                else
                {
                    if (zucmotStatus->funcIOMap.stat_boot_do[i].type >= 0 && zucmotStatus->funcIOMap.stat_boot_do[i].index >= 0)
                    {
                        FD_CLR(zucmotStatus->funcIOMap.stat_boot_do[i].index,
                               &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotStatus->funcIOMap.stat_boot_do[i].type]);
                    }
                }
                // stat_emergency_stop_do
                if (zucmotCommand->setfuncDIO.funcIOMap.stat_emergency_stop_do[i].type >= 0 &&
                    zucmotCommand->setfuncDIO.funcIOMap.stat_emergency_stop_do[i].index >= 0)
                {
                    FD_SET(zucmotCommand->setfuncDIO.funcIOMap.stat_emergency_stop_do[i].index,
                           &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotCommand->setfuncDIO.funcIOMap.stat_emergency_stop_do[i].type]);
                }
                else
                {
                    if (zucmotStatus->funcIOMap.stat_emergency_stop_do[i].type >= 0 && zucmotStatus->funcIOMap.stat_emergency_stop_do[i].index >= 0)
                    {
                        FD_CLR(zucmotStatus->funcIOMap.stat_emergency_stop_do[i].index,
                               &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotStatus->funcIOMap.stat_emergency_stop_do[i].type]);
                    }
                }
                // stat_percentage_mode_level2_do
                if (zucmotCommand->setfuncDIO.funcIOMap.stat_percentage_mode_level2_do[i].type >= 0 &&
                    zucmotCommand->setfuncDIO.funcIOMap.stat_percentage_mode_level2_do[i].index >= 0)
                {
                    FD_SET(zucmotCommand->setfuncDIO.funcIOMap.stat_percentage_mode_level2_do[i].index,
                           &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotCommand->setfuncDIO.funcIOMap.stat_percentage_mode_level2_do[i].type]);
                }
                else
                {
                    if (zucmotStatus->funcIOMap.stat_percentage_mode_level2_do[i].type >= 0 &&
                        zucmotStatus->funcIOMap.stat_percentage_mode_level2_do[i].index >= 0)
                    {
                        FD_CLR(zucmotStatus->funcIOMap.stat_percentage_mode_level2_do[i].index,
                               &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotStatus->funcIOMap.stat_percentage_mode_level2_do[i].type]);
                    }
                }
                // stat_sys_protective_stop_do
                if (zucmotCommand->setfuncDIO.funcIOMap.stat_sys_protective_stop_do[i].type >= 0 &&
                    zucmotCommand->setfuncDIO.funcIOMap.stat_sys_protective_stop_do[i].index >= 0)
                {
                    FD_SET(zucmotCommand->setfuncDIO.funcIOMap.stat_sys_protective_stop_do[i].index,
                           &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotCommand->setfuncDIO.funcIOMap.stat_sys_protective_stop_do[i].type]);
                }
                else
                {
                    if (zucmotStatus->funcIOMap.stat_sys_protective_stop_do[i].type >= 0 && zucmotStatus->funcIOMap.stat_sys_protective_stop_do[i].index >= 0)
                    {
                        FD_CLR(zucmotStatus->funcIOMap.stat_sys_protective_stop_do[i].index,
                               &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotStatus->funcIOMap.stat_sys_protective_stop_do[i].type]);
                    }
                }
                // stat_initial_pose_do
                if (zucmotCommand->setfuncDIO.funcIOMap.stat_initial_pose_do[i].type >= 0 &&
                    zucmotCommand->setfuncDIO.funcIOMap.stat_initial_pose_do[i].index >= 0)
                {
                    FD_SET(zucmotCommand->setfuncDIO.funcIOMap.stat_initial_pose_do[i].index,
                           &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotCommand->setfuncDIO.funcIOMap.stat_initial_pose_do[i].type]);
                }
                else
                {
                    if (zucmotStatus->funcIOMap.stat_initial_pose_do[i].type >= 0 && zucmotStatus->funcIOMap.stat_initial_pose_do[i].index >= 0)
                    {
                        FD_CLR(zucmotStatus->funcIOMap.stat_initial_pose_do[i].index,
                               &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotStatus->funcIOMap.stat_initial_pose_do[i].type]);
                    }
                }
                // stat_percentage_mode_level1_do
                if (zucmotCommand->setfuncDIO.funcIOMap.stat_percentage_mode_level1_do[i].type >= 0 &&
                    zucmotCommand->setfuncDIO.funcIOMap.stat_percentage_mode_level1_do[i].index >= 0)
                {
                    FD_SET(zucmotCommand->setfuncDIO.funcIOMap.stat_percentage_mode_level1_do[i].index,
                           &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotCommand->setfuncDIO.funcIOMap.stat_percentage_mode_level1_do[i].type]);
                }
                else
                {
                    if (zucmotStatus->funcIOMap.stat_percentage_mode_level1_do[i].type >= 0 &&
                        zucmotStatus->funcIOMap.stat_percentage_mode_level1_do[i].index >= 0)
                    {
                        FD_CLR(zucmotStatus->funcIOMap.stat_percentage_mode_level1_do[i].index,
                               &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotStatus->funcIOMap.stat_percentage_mode_level1_do[i].type]);
                    }
                }
                // stat_drag_mode_do
                if (zucmotCommand->setfuncDIO.funcIOMap.stat_percentage_mode_level1_do[i].type >= 0 &&
                    zucmotCommand->setfuncDIO.funcIOMap.stat_percentage_mode_level1_do[i].index >= 0)
                {
                    FD_SET(zucmotCommand->setfuncDIO.funcIOMap.stat_percentage_mode_level1_do[i].index,
                           &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotCommand->setfuncDIO.funcIOMap.stat_percentage_mode_level1_do[i].type]);
                }
                else
                {
                    if (zucmotStatus->funcIOMap.stat_percentage_mode_level1_do[i].type >= 0 &&
                        zucmotStatus->funcIOMap.stat_percentage_mode_level1_do[i].index >= 0)
                    {
                        FD_CLR(zucmotStatus->funcIOMap.stat_percentage_mode_level1_do[i].index,
                               &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotStatus->funcIOMap.stat_percentage_mode_level1_do[i].type]);
                    }
                }
                // stat_collision_mode_do
                if (zucmotCommand->setfuncDIO.funcIOMap.stat_collision_mode_do[i].type >= 0 &&
                    zucmotCommand->setfuncDIO.funcIOMap.stat_collision_mode_do[i].index >= 0)
                {
                    FD_SET(zucmotCommand->setfuncDIO.funcIOMap.stat_collision_mode_do[i].index,
                           &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotCommand->setfuncDIO.funcIOMap.stat_collision_mode_do[i].type]);
                }
                else
                {
                    if (zucmotStatus->funcIOMap.stat_collision_mode_do[i].type >= 0 && zucmotStatus->funcIOMap.stat_collision_mode_do[i].index >= 0)
                    {
                        FD_CLR(zucmotStatus->funcIOMap.stat_collision_mode_do[i].index,
                               &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotStatus->funcIOMap.stat_collision_mode_do[i].type]);
                    }
                }
                // stat_manual_mode_do
                if (zucmotCommand->setfuncDIO.funcIOMap.stat_manual_mode_do[i].type >= 0 &&
                    zucmotCommand->setfuncDIO.funcIOMap.stat_manual_mode_do[i].index >= 0)
                {
                    FD_SET(zucmotCommand->setfuncDIO.funcIOMap.stat_manual_mode_do[i].index,
                           &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotCommand->setfuncDIO.funcIOMap.stat_manual_mode_do[i].type]);
                }
                else
                {
                    if (zucmotStatus->funcIOMap.stat_manual_mode_do[i].type >= 0 && zucmotStatus->funcIOMap.stat_manual_mode_do[i].index >= 0)
                    {
                        FD_CLR(zucmotStatus->funcIOMap.stat_manual_mode_do[i].index,
                               &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotStatus->funcIOMap.stat_manual_mode_do[i].type]);
                    }
                }
                // stat_auto_mode_do
                if (zucmotCommand->setfuncDIO.funcIOMap.stat_auto_mode_do[i].type >= 0 && zucmotCommand->setfuncDIO.funcIOMap.stat_auto_mode_do[i].index >= 0)
                {
                    FD_SET(zucmotCommand->setfuncDIO.funcIOMap.stat_auto_mode_do[i].index,
                           &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotCommand->setfuncDIO.funcIOMap.stat_auto_mode_do[i].type]);
                }
                else
                {
                    if (zucmotStatus->funcIOMap.stat_auto_mode_do[i].type >= 0 && zucmotStatus->funcIOMap.stat_auto_mode_do[i].index >= 0)
                    {
                        FD_CLR(zucmotStatus->funcIOMap.stat_auto_mode_do[i].index,
                               &zucmotStatus->funcIOMap.digtalOutput_bound[zucmotStatus->funcIOMap.stat_auto_mode_do[i].type]);
                    }
                }

                zucmotStatus->funcIOMap.task_idle_do[i] = zucmotCommand->setfuncDIO.funcIOMap.task_idle_do[i];
                zucmotStatus->funcIOMap.task_paused_do[i] = zucmotCommand->setfuncDIO.funcIOMap.task_paused_do[i];
                zucmotStatus->funcIOMap.task_running_do[i] = zucmotCommand->setfuncDIO.funcIOMap.task_running_do[i];
                zucmotStatus->funcIOMap.stat_fault_do[i] = zucmotCommand->setfuncDIO.funcIOMap.stat_fault_do[i];
                zucmotStatus->funcIOMap.stat_poweron_do[i] = zucmotCommand->setfuncDIO.funcIOMap.stat_poweron_do[i];
                zucmotStatus->funcIOMap.stat_enabled_do[i] = zucmotCommand->setfuncDIO.funcIOMap.stat_enabled_do[i];
                zucmotStatus->funcIOMap.stat_motion_do[i] = zucmotCommand->setfuncDIO.funcIOMap.stat_motion_do[i];
                zucmotStatus->funcIOMap.stat_static_do[i] = zucmotCommand->setfuncDIO.funcIOMap.stat_static_do[i];
                zucmotStatus->funcIOMap.stat_boot_do[i] = zucmotCommand->setfuncDIO.funcIOMap.stat_boot_do[i];
                zucmotStatus->funcIOMap.stat_emergency_stop_do[i] = zucmotCommand->setfuncDIO.funcIOMap.stat_emergency_stop_do[i];
                zucmotStatus->funcIOMap.stat_percentage_mode_level2_do[i] = zucmotCommand->setfuncDIO.funcIOMap.stat_percentage_mode_level2_do[i];
                zucmotStatus->funcIOMap.stat_sys_protective_stop_do[i] = zucmotCommand->setfuncDIO.funcIOMap.stat_sys_protective_stop_do[i];
                zucmotStatus->funcIOMap.stat_initial_pose_do[i] = zucmotCommand->setfuncDIO.funcIOMap.stat_initial_pose_do[i];
                zucmotStatus->funcIOMap.stat_percentage_mode_level1_do[i] = zucmotCommand->setfuncDIO.funcIOMap.stat_percentage_mode_level1_do[i];
                zucmotStatus->funcIOMap.stat_drag_mode_do[i] = zucmotCommand->setfuncDIO.funcIOMap.stat_drag_mode_do[i];
                zucmotStatus->funcIOMap.stat_collision_mode_do[i] = zucmotCommand->setfuncDIO.funcIOMap.stat_collision_mode_do[i];
                zucmotStatus->funcIOMap.stat_manual_mode_do[i] = zucmotCommand->setfuncDIO.funcIOMap.stat_manual_mode_do[i];
                zucmotStatus->funcIOMap.stat_auto_mode_do[i] = zucmotCommand->setfuncDIO.funcIOMap.stat_auto_mode_do[i];
            }
            break;
        }

        case ZUCMOT_SET_SAFETY_FUNCDI_MAP: {
            // if (zucmotConfig->cab_type == 2)
            // {
            //     unsigned int sfunc_cfg = 0;
            //     unsigned int ori_sfunc;
            //     ori_sfunc = get_scb_safety_di_cfg();
            //     for (int i = 0; i < 8; i++) { sfunc_cfg += (zucmotCommand->safetyFuncDIs.sFuncDISettings.diChnFunc[7 - i] << 4 * i); }
            //     set_scb_safety_DI_cfg(sfunc_cfg);

            //     MOTCMD_INFO("scb safety function di: %d\n", ori_sfunc);
            //     //command 中 channel 1→8: ch4 ch3 ch2 ch1 ch8 ch7 ch6 ch5
            //     //对顺序进行调整: ch1 ch2 ch3 ch4 ch5 ch6 ch7 ch8
            //     zucmotConfig->safetyCfg.sFuncDISettings.diChnFunc[0] = zucmotCommand->safetyFuncDIs.sFuncDISettings.diChnFunc[3];
            //     zucmotConfig->safetyCfg.sFuncDISettings.diChnFunc[1] = zucmotCommand->safetyFuncDIs.sFuncDISettings.diChnFunc[2];
            //     zucmotConfig->safetyCfg.sFuncDISettings.diChnFunc[2] = zucmotCommand->safetyFuncDIs.sFuncDISettings.diChnFunc[1];
            //     zucmotConfig->safetyCfg.sFuncDISettings.diChnFunc[3] = zucmotCommand->safetyFuncDIs.sFuncDISettings.diChnFunc[0];
            //     zucmotConfig->safetyCfg.sFuncDISettings.diChnFunc[4] = zucmotCommand->safetyFuncDIs.sFuncDISettings.diChnFunc[7];
            //     zucmotConfig->safetyCfg.sFuncDISettings.diChnFunc[5] = zucmotCommand->safetyFuncDIs.sFuncDISettings.diChnFunc[6];
            //     zucmotConfig->safetyCfg.sFuncDISettings.diChnFunc[6] = zucmotCommand->safetyFuncDIs.sFuncDISettings.diChnFunc[5];
            //     zucmotConfig->safetyCfg.sFuncDISettings.diChnFunc[7] = zucmotCommand->safetyFuncDIs.sFuncDISettings.diChnFunc[4];

            //     zucmotConfig->safetyCfg.sFuncDISettings.drag_limit = zucmotCommand->safetyFuncDIs.sFuncDISettings.drag_limit;
            //     zucmotStatus->sFuncDISettings = zucmotConfig->safetyCfg.sFuncDISettings;

            //     unsigned int ori_crc = zucmotStatus->safetyCRC;
            //     zucmotStatus->safetyCRC = crc32((const UINT8*)&zucmotConfig->safetyCfg, sizeof(SafetyConfig));
            //     if (ori_sfunc != sfunc_cfg && task_init_finish_flag == 1)
            //     {
            //         char str[MAX_ERROR_MSG_STR_LEN];
            //         char hex_a[9];          // 存储a的16进制表示（包括终止符）
            //         char hex_b[9];          // 存储b的16进制表示（包括终止符）
            //         int diff_positions[8];  // 存储不同位置的索引
            //         int di_id = 0xff;
            //         snprintf(hex_a, sizeof(hex_a), "%08x", ori_sfunc);  // 将a转换为16进制字符串
            //         snprintf(hex_b, sizeof(hex_b), "%08x", sfunc_cfg);  // 将b转换为16进制字符串
            //         for (int i = 0; i < 8; i++)
            //         {
            //             //   i:       0    1    2    3    4     5    6    7
            //             // ch:     ch4 ch3 ch2 ch1 ch8 ch7 ch6 ch5
            //             if (hex_a[i] != hex_b[i])
            //             {
            //                 if (i < 4)
            //                 {
            //                     di_id = 4 - i;
            //                 }
            //                 else if (i < 8)
            //                 {
            //                     di_id = 4 + 8 - i;
            //                 }
            //                 snprintf(str,
            //                          256,
            //                          "{\"fixed_key\":[\"%#x\", \"%#x\", \"%d&%d\", \"%c\",\"%c\"]}",
            //                          ori_crc,
            //                          zucmotStatus->safetyCRC,
            //                          di_id,
            //                          di_id + 8,
            //                          hex_a[i],
            //                          hex_b[i]);
            //                 reportInfo(INFO_SAFETY_CONF_SAFETY_FUNCDI_MAP_CHANGED, "checksum changed # %s ", str);
            //             }
            //         }
            //     }
            // }
            break;
        }

        case ZUCMOT_SET_SAFETY_FUNCDO_MAP: {
            // if (zucmotConfig->cab_type == 2)
            // {
            //     unsigned int sfunc_cfg = 0;
            //     unsigned int ori_sfunc;
            //     ori_sfunc = get_scb_safety_do_cfg();
            //     for (int i = 0; i < 8; i++)
            //     {
            //         sfunc_cfg += (zucmotCommand->safetyFuncDOs.sFuncDOSettings.doChnFunc[7 - i] << 4 * i);
            //         if (zucmotCommand->safetyFuncDOs.sFuncDOSettings.doChnFunc[i] > 1)  // 配置了安全IO
            //         {
            //             int index = (3 + 8 - i) % 8;
            //             FD_SET(index, &zucmotStatus->funcIOMap.digtalOutput_bound[0]);
            //             FD_SET(index + 8, &zucmotStatus->funcIOMap.digtalOutput_bound[0]);
            //         }
            //         else
            //         {
            //             int index = (3 + 8 - i) % 8;
            //             FD_CLR(index, &zucmotStatus->funcIOMap.digtalOutput_bound[0]);
            //             FD_CLR(index + 8, &zucmotStatus->funcIOMap.digtalOutput_bound[0]);
            //         }
            //     }
            //     set_scb_safety_DO_cfg(sfunc_cfg);
            //     MOTCMD_INFO("scb safety function do: %d\n", sfunc_cfg);
            //     //command 中 channel 1→8: ch4 ch3 ch2 ch1 ch8 ch7 ch6 ch5
            //     //对顺序进行调整: ch1 ch2 ch3 ch4 ch5 ch6 ch7 ch8
            //     zucmotConfig->safetyCfg.sFuncDOSettings.doChnFunc[0] = zucmotCommand->safetyFuncDOs.sFuncDOSettings.doChnFunc[3];
            //     zucmotConfig->safetyCfg.sFuncDOSettings.doChnFunc[1] = zucmotCommand->safetyFuncDOs.sFuncDOSettings.doChnFunc[2];
            //     zucmotConfig->safetyCfg.sFuncDOSettings.doChnFunc[2] = zucmotCommand->safetyFuncDOs.sFuncDOSettings.doChnFunc[1];
            //     zucmotConfig->safetyCfg.sFuncDOSettings.doChnFunc[3] = zucmotCommand->safetyFuncDOs.sFuncDOSettings.doChnFunc[0];
            //     zucmotConfig->safetyCfg.sFuncDOSettings.doChnFunc[4] = zucmotCommand->safetyFuncDOs.sFuncDOSettings.doChnFunc[7];
            //     zucmotConfig->safetyCfg.sFuncDOSettings.doChnFunc[5] = zucmotCommand->safetyFuncDOs.sFuncDOSettings.doChnFunc[6];
            //     zucmotConfig->safetyCfg.sFuncDOSettings.doChnFunc[6] = zucmotCommand->safetyFuncDOs.sFuncDOSettings.doChnFunc[5];
            //     zucmotConfig->safetyCfg.sFuncDOSettings.doChnFunc[7] = zucmotCommand->safetyFuncDOs.sFuncDOSettings.doChnFunc[4];
            //     zucmotStatus->sFuncDOSettings = zucmotConfig->safetyCfg.sFuncDOSettings;
            //     unsigned int ori_crc = zucmotStatus->safetyCRC;
            //     zucmotStatus->safetyCRC = crc32((const UINT8*)&zucmotConfig->safetyCfg, sizeof(SafetyConfig));
            //     if (ori_sfunc != sfunc_cfg && task_init_finish_flag == 1)
            //     {
            //         char str[MAX_ERROR_MSG_STR_LEN];
            //         char hex_a[9];          // 存储a的16进制表示（包括终止符）
            //         char hex_b[9];          // 存储b的16进制表示（包括终止符）
            //         int diff_positions[8];  // 存储不同位置的索引
            //         int do_id = 0xff;
            //         snprintf(hex_a, sizeof(hex_a), "%08x", ori_sfunc);  // 将a转换为16进制字符串
            //         snprintf(hex_b, sizeof(hex_b), "%08x", sfunc_cfg);  // 将b转换为16进制字符串
            //         for (int i = 0; i < 8; i++)
            //         {
            //             //   i:       0    1    2    3    4     5    6    7
            //             // ch:     ch4 ch3 ch2 ch1 ch8 ch7 ch6 ch5
            //             if (hex_a[i] != hex_b[i])
            //             {
            //                 if (i < 4)
            //                 {
            //                     do_id = 4 - i;
            //                 }
            //                 else if (i < 8)
            //                 {
            //                     do_id = 4 + 8 - i;
            //                 }
            //                 snprintf(str,
            //                          256,
            //                          "{\"fixed_key\":[\"%#x\", \"%#x\", \"%d&%d\", \"%c\",\"%c\"]}",
            //                          ori_crc,
            //                          zucmotStatus->safetyCRC,
            //                          do_id,
            //                          do_id + 8,
            //                          hex_a[i],
            //                          hex_b[i]);
            //                 reportInfo(INFO_SAFETY_CONF_SAFETY_FUNCDO_MAP_CHANGED, "checksum changed # %s ", str);
            //             }
            //         }
            //     }
            // }
            break;
        }

        case ZUCMOT_SETUP_EXTIO: {
            // int sem_id = sem_get(EXTIO_SEM_KEY);
            // if (sem_p(sem_id) == 0)
            // {
            //     if (zucmotExtIOStruct->extIOShmStruct.extIOStat.status == 0)
            //     {
            //         zucmotExtIOStruct->extIOShmStruct.extIOModCount = zucmotCommand->extIOsetup.extIOModCnt;
            //         int _baudrate = -1;
            //         int _databits = -1;
            //         int _stopBits = -1;
            //         int _parity = -1;
            //         int _rtuBusParamatersInit = 0;
            //         for (int i = 0; i < zucmotCommand->extIOsetup.extIOModCnt; ++i)
            //         {
            //             zucmotExtIOStruct->extIOShmStruct.extIOModConfig[i] = zucmotCommand->extIOsetup.extIOModConfig[i];
            //             if (zucmotCommand->extIOsetup.extIOModConfig[i].extIOSetup.extType == 0)  /// RTU  记录第一个Extio模块的总线参数
            //             {
            //                 if (_rtuBusParamatersInit == 0)
            //                 {
            //                     _rtuBusParamatersInit = 1;
            //                     _baudrate = zucmotCommand->extIOsetup.extIOModConfig[i].extIOSetup.commInfo.rtuComm.baudrate;
            //                     _databits = zucmotCommand->extIOsetup.extIOModConfig[i].extIOSetup.commInfo.rtuComm.databits;
            //                     _stopBits = zucmotCommand->extIOsetup.extIOModConfig[i].extIOSetup.commInfo.rtuComm.stopbits;
            //                     _parity = zucmotCommand->extIOsetup.extIOModConfig[i].extIOSetup.commInfo.rtuComm.parity;
            //                 }
            //                 else if (_baudrate != zucmotCommand->extIOsetup.extIOModConfig[i].extIOSetup.commInfo.rtuComm.baudrate ||
            //                          _databits != zucmotCommand->extIOsetup.extIOModConfig[i].extIOSetup.commInfo.rtuComm.databits ||
            //                          _stopBits != zucmotCommand->extIOsetup.extIOModConfig[i].extIOSetup.commInfo.rtuComm.stopbits ||
            //                          _parity != zucmotCommand->extIOsetup.extIOModConfig[i]
            //                                         .extIOSetup.commInfo.rtuComm.parity)  //  其他Extio  RTU 模块的总线参数与地一个模块的总线参数不一致
            //                 {
            //                     reportWarning(
            //                         EXTIO_WARNING_INIT_RTU_PARAMETERS,
            //                         "Multiple RTU extension IO modules are configured and the serial bus parameters of each module are inconsistent.");
            //                 }
            //             }
            //         }
            //     }
            //     sem_v(sem_id);
            // }
            break;
        }

        case ZUCMOT_SETMODE_EXTIO: {
            // int sem_id = sem_get(EXTIO_SEM_KEY);
            // if (sem_p(sem_id) == 0)
            // {
            //     zucmotExtIOStruct->extIOShmStruct.mode = zucmotCommand->extIOsetup.mode;
            //     sem_v(sem_id);
            // }
            break;
        }

        case ZUCMOT_SETRECONNECT_EXTIO: {
            // int sem_id = sem_get(EXTIO_SEM_KEY);
            // if (sem_p(sem_id) == 0)
            // {
            //     zucmotExtIOStruct->extIOShmStruct.reConnectInterval = zucmotCommand->extIOsetup.reConnectInterval;
            //     zucmotExtIOStruct->extIOShmStruct.reConnectMaxTimes = zucmotCommand->extIOsetup.reConnectMaxTimes;
            //     sem_v(sem_id);
            // }
            break;
        }

        case ZUCMOT_MBTIO_ADD_SIGNAL: {
            if (zucmotCommand->tioplsAddSig.chnId < 0 || zucmotCommand->tioplsAddSig.chnId > 1 ||
                zucmotConfig->tioConfig.rsChnCfg[zucmotCommand->tioplsAddSig.chnId].mode != 0 || zucmotCommand->tioplsAddSig.type < 1 ||
                zucmotCommand->tioplsAddSig.type > 4)
            {
                reportError(TIO_INVALID_SIGNAL_DEFINITION,
                            "failed add modbus signal due to invalid parameter, %d, %d, %d",
                            zucmotCommand->tioplsAddSig.chnId,
                            zucmotConfig->tioConfig.rsChnCfg[zucmotCommand->tioplsAddSig.chnId].mode,
                            zucmotCommand->tioplsAddSig.type);
                break;
            }
            ModRtuComm rtuComm = zucmotConfig->tioConfig.rsChnCfg[zucmotCommand->tioplsAddSig.chnId].rtuComm;
            MOTCMD_INFO("chn %d, slave Id = %d\n", zucmotCommand->tioplsAddSig.chnId, rtuComm.slaveId);
            if (0 != tio_gripper_add_signal((const char*)(zucmotCommand->tioplsAddSig.sigName),
                                            zucmotCommand->tioplsAddSig.chnId,
                                            rtuComm.slaveId,
                                            zucmotCommand->tioplsAddSig.type,
                                            zucmotCommand->tioplsAddSig.addr))
            {
                reportError(TIO_SIGNAL_EXCEED_MAX_NUM, "signals defined exceed the maximum numbers");
                break;
            }
            GripperStateUnit* grrpperState = tio_gripper_list_signal();
            for (int i = 0; i < MAX_TIO_SIG_NUM; i++) { zucmotStatus->gripperState[i] = grrpperState[i]; }
            break;
        }

        case ZUCMOT_MBTIO_DEL_SIGNAL: {
            tio_gripper_del_signal((const char*)(zucmotCommand->tioplsDelSig.sigName));
            // 若无该信号量则无需报错，以避免app增加无效信号时虽控制器报错但仍然在app上增加成功，导致无法删除的问题（禅道BUG5629）
            GripperStateUnit* gripperState = tio_gripper_list_signal();
            for (int i = 0; i < MAX_TIO_SIG_NUM; i++) { zucmotStatus->gripperState[i] = gripperState[i]; }
            break;
        }

        case ZUCMOT_MBTIO_UPDATE_SIGNAL: {
            // if (getTioCommState())
            // {
            //     setTioCommState(0);
            // }
            // int chn_mode[2] = {get_Rs485_chn_mode(0), get_Rs485_chn_mode(1)};

            // if (tio_gripper_update_signal((const char*)(zucmotCommand->tioplsUpdateSig.sigName), zucmotCommand->tioplsUpdateSig.freq, chn_mode) != 0)
            // {
            //     reportError(TIO_SIGNAL_NOT_EXIST, "specified signal doesn't exist");
            //     zucmotStatus->commandStatus = ZUCMOT_COMMAND_BAD_EXEC;
            //            //     // tpManager_tpabort();
            //     // SET_MOTION_ERROR_FLAG(1);
            // }
            break;
        }

        case ZUCMOT_MBTIO_SEND_COMMAND: {
            // MOTCMD_INFO(
            //     "ZUCMOT_MBTIO_SEND_COMMAND chnId = %d mode = %d\n", zucmotCommand->tioplsSendCmd.chnId, get_Rs485_chn_mode(zucmotCommand->tioplsSendCmd.chnId));
            // if (zucmotConfig->tioConfig.rsChnCfg[zucmotCommand->tioplsSendCmd.chnId].mode == 2)
            // {
            //     reportError(TIO_RSCMD_ERROR_CHN, "failed to send rs command since this chnneal is only for torque sensor");
            //     break;
            // }
            // if (getTioCommState())
            // {
            //     setTioCommState(0);
            // }
            // if (tio_gripper_send(zucmotCommand->tioplsSendCmd.chnId, zucmotCommand->tioplsSendCmd.cmd, zucmotCommand->tioplsSendCmd.len) != 0)
            // {
            //     reportError(TIO_RSCMD_QUEUE_FULL, "failed to send rs command since command queue was full");
            //     zucmotStatus->commandStatus = ZUCMOT_COMMAND_BAD_EXEC;
            //            //     tpManager_tpabort();
            //     SET_MOTION_ERROR_FLAG(1);
            //     tio_cmd_reset();
            // }
            break;
        }

            /* needed for synchronous I/O */
        case ZUCMOT_SET_AOUT: {
            MOTCMD_DEBUG("SET_AOUT");
            if (zucmotCommand->motsetADout.now)
            {  //we set it right away
                zucmot_SetAO((IODevType)zucmotCommand->motsetADout.ioType, zucmotCommand->motsetADout.out, zucmotCommand->motsetADout.minLimit);
            }
            else  // 运动中设置AO
            {     // we put it on the TP queue, warning: only room for one in there, any new ones will overwrite
                  //                tpManager_tpSetAout(zucmotCommand->motsetADout.ioType,
                  // zucmotCommand->motsetADout.out,
                  // zucmotCommand->motsetADout.minLimit,
                  // zucmotCommand->motsetADout.maxLimit);
            }
            break;
        }

        case ZUCMOT_SET_DOUT: {
            MOTCMD_DEBUG("SET_DOUT");
            // if (zucmotCommand->motsetADout.now)
            // {  //we set it right away
            //     if (zucmotCommand->motsetADout.ioType == 0)
            //     {  // basic IO
            //         write_DIO_i((IODevType)zucmotCommand->motsetADout.ioType, zucmotCommand->motsetADout.out, zucmotCommand->motsetADout.start);
            //     }
            //     else if (zucmotCommand->motsetADout.ioType == 1)
            //     {  // tool IO
            //         tio_set_dout(zucmotCommand->motsetADout.out, zucmotCommand->motsetADout.start);
            //     }
            //     else if (zucmotCommand->motsetADout.ioType == 2)
            //     {  // ext IO
            //         write_DIO_i((IODevType)zucmotCommand->motsetADout.ioType, zucmotCommand->motsetADout.out, zucmotCommand->motsetADout.start);
            //     }
            //     else if (zucmotCommand->motsetADout.ioType == 3)
            //     {
            //     }
            //     else if (zucmotCommand->motsetADout.ioType == 4)
            //     {  // Modbus slave IO
            //         write_DIO_i((IODevType)zucmotCommand->motsetADout.ioType, zucmotCommand->motsetADout.out, zucmotCommand->motsetADout.start);
            //     }
            //     else if (zucmotCommand->motsetADout.ioType == 5)
            //     {  // profinet slave IO
            //         write_DIO_i((IODevType)zucmotCommand->motsetADout.ioType, zucmotCommand->motsetADout.out, zucmotCommand->motsetADout.start);
            //     }
            //     else if (zucmotCommand->motsetADout.ioType == 6)
            //     {  // eip slave IO
            //         write_DIO_i((IODevType)zucmotCommand->motsetADout.ioType, zucmotCommand->motsetADout.out, zucmotCommand->motsetADout.start);
            //     }
            // }
            // else  // 运动中设置DO
            // {     // we put it on the TP queue, warning: only room for one in there, any new ones will overwrite
            //            //     tpManager_tpSetDout(
            //         zucmotCommand->motsetADout.ioType, zucmotCommand->motsetADout.out, zucmotCommand->motsetADout.start, zucmotCommand->motsetADout.end);
            // }
            break;
        }

        case ZUCMOT_SERVO_SET_CAPTURE: {
            zucmotConfig->captureSettings[zucmotCommand->setcapture.captId].stateType =
                zucmotCommand->setcapture.stateToCapt;  // 0: joint position, 1: cartesian position
            zucmotConfig->captureSettings[zucmotCommand->setcapture.captId].trigger = zucmotCommand->setcapture.trigger;
            zucmotStatus->captureStatus[zucmotCommand->setcapture.captId].captStatus = 1;  // start capturing
            MOTCMD_INFO("captStatus=%d, triggr: %d, %d, %d\n",
                        1,
                        zucmotCommand->setcapture.trigger.ioType,
                        zucmotCommand->setcapture.trigger.ioIndex,
                        zucmotCommand->setcapture.trigger.trigEvent);
            zucmot_config_change();
            break;
        }

        case ZUCMOT_SERVO_CLEAR_CAPTURE: {
            zucmotStatus->captureStatus[zucmotCommand->clearcapture.captId].captStatus = 0;  // stop capturing
            MOTCMD_INFO("capture cleared: %d\n", zucmotCommand->clearcapture.captId);
            break;
        }

        default: {
            reportError(UNKNOWN_ERROR, _("unrecognized command %d"), zucmotCommand->command);
            zucmotStatus->commandStatus = ZUCMOT_COMMAND_UNKNOWN_COMMAND;
            break;
        }
        } /* end of: command switch */
        if (zucmotStatus->commandStatus != ZUCMOT_COMMAND_OK)
        {
            MOTCMD_DEBUG("ERROR: %d", zucmotStatus->commandStatus);
        }
        /* synch tail count */
        zucmotStatus->tail = zucmotStatus->head;
        zucmotConfig->tail = zucmotConfig->head;
        zucmotDebug->tail = zucmotDebug->head;
    }
    /* end of: if-new-command */
    return;
}
